#!/usr/bin/python
# -*- coding: UTF-8 -*-

import time,threading,sys
sys.path.append("/home/biowin/Minibot/04.RobotSamples/03.SpiderRobot")
from BwRobotLib import BwRobotLib
from SpiderRobot import SpiderRobot
from BwSensorLib import BwSensorLib
import os,inspect

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"


class CrossFire:
	def __init__(self):
		# 实例化传感器库对象
		self.sensorlib = BwsensorliblibLib("udp")
		# 实例化BwRobotLib库
		self.robotlib = BwRobotLib("udp")
		# 实例化蜘蛛机器人库
		self.spider = SpiderRobot(self.robotlib)
		# 连接机器人设备
		self.robotlib.connectRobot("10.10.100.254",8899) #真机AP模式
        # 导入机器人配置参数
        self.robotlib.loadConfig(config_file)
		#配置ID
		self.spider.bindIds(self.robotlib.module_ids)
		# 时间变量
		self.wait_time = 0.9
		# 蜘蛛机器人运动标志位
		self.flag_threading = 0 
		# 点阵显示
		# 委屈　左眨眼　右眨眼　
		self.display_data = [[0x63,0x14,0x22,0x00,0x3E],[0x20,0x12,0x20,0x08,0x1C],[0x02,0x24,0x02,0x08,0x1C]]


	# 点阵显示表情
	def crossFireDisPlay(self):
		while 1:
			if(self.flag_threading==0):
				self.sensorlib.setMatrixSensor(self.display_data[1]) # 点阵显示左眨眼图案
				time.sleep(self.wait_time)
				self.sensorlib.setMatrixSensor(self.display_data[2]) # 点阵显示右眨眼图案
				time.sleep(self.wait_time)
			else:
				self.sensorlib.setMatrixSensor(self.display_data[0]) # 点阵显示委屈图案
				print("display")
				time.sleep(self.wait_time)

	# 超声波传感器测量距离
	def crossFireMeasure(self):
		while 1:
			ultrasonic = self.sensorlib.readUltrasonicSensor() # 读取超声波传感器数据
			# 0-25cm有障碍物
			if (ultrasonic<=25 and ultrasonic>0):	
				self.flag_threading = 1 # 匍匐前进标志位
				print("仿生蜘蛛匍匐前进")


	# 穿越火线
	def crossFireMotion(self):
		# 创建线程对象
		self.threading_1 = threading.Thread(target=self.crossFireDisPlay, args=())       # 实例化点阵显示线程
		self.threading_2 = threading.Thread(target=self.crossFireMeasure, args=())	     # 实例化超声波传感器检测线程
		# 开启线程
		self.threading_1.start() 
		self.threading_2.start()
		while 1:
			if(self.flag_threading==0): # 检测到有障碍物
				self.spider.goForward() # 正常前进
			else:
				for a in range (0,5):
					self.spider.goForward_down() # 匍匐前进
				self.flag_threading = 0
			print("over")	



# 主函数
def main():
    # 实例化穿越火线类
    crossFire = CrossFire()
    # 执行穿越火线功能
    crossFire.crossFireMotion()


if __name__ == "__main__":
    try:
        main()
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass